#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>

#include <deque>

class PointCloudAccumulator
{
public:
    PointCloudAccumulator(ros::NodeHandle &nh)
        : tf_listener_(tf_buffer_)
    {
        ros::NodeHandle pnh("~");

        pnh.param<std::string>("pc_input_topic", pc_input_topic_, "/cloud_registered");
        pnh.param<std::string>("pc_output_topic", pc_output_topic_, "/pc_accumulate");
        pnh.param<std::string>("world_frame_id", world_frame_id_, "map");
        pnh.param<std::string>("body_frame_id", body_frame_id_, "body");
        pnh.param<int>("history_length", history_length_, 5);

        sub_ = nh.subscribe(pc_input_topic_, 10, &PointCloudAccumulator::pointCloudCallback, this);
        pub_ = nh.advertise<sensor_msgs::PointCloud2>(pc_output_topic_, 1);
    }

private:
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
    {
        sensor_msgs::PointCloud2 cloud_transformed;

        std::string from_frame = cloud_msg->header.frame_id;

        if (from_frame != world_frame_id_)
        {
            try
            {
                geometry_msgs::TransformStamped transform_stamped = tf_buffer_.lookupTransform(
                    world_frame_id_, cloud_msg->header.frame_id,
                    cloud_msg->header.stamp, ros::Duration(0.2));

                tf2::doTransform(*cloud_msg, cloud_transformed, transform_stamped);
            }
            catch (tf2::TransformException &ex)
            {
                ROS_WARN("Transform failed: %s", ex.what());
                return;
            }
        }
        else
        {
            cloud_transformed = *cloud_msg;
        }

        cloud_queue_.push_back(cloud_transformed);

        if (cloud_queue_.size() > history_length_)
        {
            cloud_queue_.pop_front();
        }

        // 合并点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr accumulated_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        for (const auto &cloud : cloud_queue_)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>());
            pcl::fromROSMsg(cloud, *temp_cloud);
            *accumulated_cloud += *temp_cloud;
        }

        sensor_msgs::PointCloud2 output_msg;
        pcl::toROSMsg(*accumulated_cloud, output_msg);
        output_msg.header.frame_id = world_frame_id_;
        output_msg.header.stamp = ros::Time::now();

        pub_.publish(output_msg);
    }

    ros::Subscriber sub_;
    ros::Publisher pub_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    std::string pc_input_topic_;
    std::string pc_output_topic_;
    std::string world_frame_id_;
    std::string body_frame_id_;
    int history_length_;

    std::deque<sensor_msgs::PointCloud2> cloud_queue_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcd_accumulate");
    ros::NodeHandle nh;

    PointCloudAccumulator accumulator(nh);

    ros::spin();
    return 0;
}
